#include "Arduino.h"
#include "DFRduino.h"

DFRduino::DFRduino(int check_digital_keys) {
	dfrduino_enable_digital = check_digital_keys;
}

/**
 * ===========================================================
 *  DFRduino Key Method Implementations
 * ===========================================================
 */

// public
void enable_digital_buttons()
{
	enable_digital = 1;
}

// public
void disable_digital_buttons()
{
	enable_digital = 0;
	oldkey = -1;
}

// public
int DFRduino::get_key_press() {
  int keyPinVal = analogRead(KEYPIN);
  int key = -1;
  // get the key
  key = convert_key(keyPinVal);    // convert into key press
  if (key != oldkey) {   // if keypress is detected
    delay(50);      // wait for debounce time
    key = convert_key(keyPinVal);    // convert into key press
    if (key != oldkey)
      oldkey = key;
  }
  return key;
}

// private
int DFRduino::convert_key(unsigned int input)
{
	int k;
	for (k = 0; k < NUM_KEYS; k++)
	{
		if (input < adc_key_val[k])
		{
			return k;
		}
	}
	if (dfrduino_enable_digital)
	{
		if (digitalRead(KEYPIN_6))
			return 6;
		if (digitalRead(KEYPIN_7))
			return 7;
	}
	return -1;
}

/**
 * ===========================================================
 *  DFRduino Motor Method Implementations
 * ===========================================================
 */

//public
void enable_motor1()
{
	enable_motor1 = 1;
}

//public
void enable_motor2()
{
	enable_motor2 = 1;
}

//public
void disable_motor1()
{
	enable_motor1 = 0;
	// set motor speed to 0
	digitalWrite(E1,LOW);
}

//public
void disable_motor2()
{
	enable_motor2 = 0;
	// set motor 2 speed to 0
	digitalWrite(E2,LOW);
}

// public
void DFRduino::motor1_go(int direction, int rate)
{
	if (enable_motor1 && (direction == MOTOR_FWD || direction == MOTOR_BKD))
	{
		analogWrite (E2, rate);    
		digitalWrite(M2, !direction);
	}
}

// public
void DFRduino::motor2_go(int direction, int rate)
{
	if (enable_motor1 && (direction == MOTOR_FWD || direction == MOTOR_BKD))
	{
		analogWrite (E1, rate);
		digitalWrite(M1, direction); 
	}
}

// public
void DFRduino::motors_stop()
{
	digitalWrite(E1,LOW);  
	digitalWrite(E2,LOW);
}
